#include "main.h"
#include "myMath.h"

Quaternion::Quaternion(void)
{
	w=0;
	v=TVector(0,0,0);
}

Quaternion::Quaternion(float _w, TVector _v)
{
	w=_w;
	v=_v;
}

Quaternion::Quaternion(float _w,float _x,float _y,float _z)
{
	w=_w;
	v._x=_x;
	v._y=_y;
	v._z=_z;
}


Quaternion::~Quaternion(void)
{
}

float Quaternion::magnitude()
{
	return sqrt(pow(w,2)+pow(v.magnitude(),2));
}

void Quaternion::normalise()
{
	float mag=this->magnitude();
	w=w/mag;
	v._x=v._x/mag;
	v._y=v._y/mag;
	v._z=v._z/mag;
}

void Quaternion::conjugate()
{
	v.invert();
}

void Quaternion::inverse()
{
	this->conjugate();
	float mag=this->magnitude();
	w=w/pow(mag,2);
	v._x=v._x/pow(mag,2);
	v._y=v._y/pow(mag,2);
	v._z=v._z/pow(mag,2);
}

bool Quaternion::isUnit()
{
	if (magnitude()==1) return true;
	return false;
}


Quaternion Quaternion::add(Quaternion q1,Quaternion q2)
{
	Quaternion q3;
	float w=q1.w+q2.w;
	TVector v=q1.v+q2.v;
	q3=Quaternion(w,v);
	//q3.normalise();
	return q3;
}

Quaternion Quaternion::multiply(Quaternion q1, Quaternion q2)
{
	float w;
	Quaternion q3;
	TVector v;
	w=q1.w*q2.w - TVector::dotProduct(q1.v,q2.v);
	v=q1.v*q2.v + TVector::multiply(q1.v,q2.w)+TVector::multiply(q2.v,q1.w);
	//(q1.v*q2.v).print();
	//TVector::multiply(q1.v,q2.w).print();
	//TVector::multiply(q2.v,q1.w).print();
	q3=Quaternion(w,v);
	//q3.normalise();
	return q3;
}

Quaternion Quaternion::scalarMultiply(float s)
{
	return Quaternion(w*s,v._x*s,v._y*s,v._z*s);
}


angleAxis Quaternion::toAngleAxis()
{
	TVector axis=v;
	float angle;
	axis.unit();
	angle=acos(w)*2*180*PI;
	return angleAxis(angle,v);
}

glMatrix Quaternion::convertToMatrix()
{
	float a11,a12,a13,a21,a22,a23,a31,a32,a33;
	float x,y,z;
	x=v._x;
	y=v._y;
	z=v._z;
	a11=1 - 2*pow(y,2)-2*pow(z,2);
	a12=2*x*y+2*w*z;
	a13=2*x*z-2*w*y;
	a21=2*x*y-2*w*z;
	a22=1-2*pow(x,2)-2*pow(z,2);
	a23=2*y*z+2*w*x;
	a31=2*x*z+2*w*y;
	a32=2*y*z-2*w*x;
	a33=1-2*pow(x,2)-2*pow(y,2);
	double m[]={a11,a12,a13,0,a21,a22,a23,0,a31,a32,a33,0,0,0,0,1};
	return glMatrix(m);
}

void Quaternion::constructFromMatrix(glMatrix M)
{
	w=sqrt(1+M._Mx[0][0]+M._Mx[1][1]+M._Mx[2][2])/2;
	x=(M._Mx[1][2]-M._Mx[2][1])/4*w;
	y=(M._Mx[2][0]-M._Mx[0][2])/4*w;
	z=(M._Mx[0][1]-M._Mx[1][0])/4*w;

}

Quaternion Quaternion::lerp(Quaternion q2,float timeStep)
{
	Quaternion q1=Quaternion(w,v);
	q1.conjugate();
	Quaternion delta=q2+q1;
	delta.w=q2.w-q1.w;
	q1.conjugate();
	delta.w   =delta.w    * timeStep;
	delta.v._x=delta.v._x * timeStep;
	delta.v._y=delta.v._y * timeStep;
	delta.v._z=delta.v._z * timeStep;
	Quaternion lerped;
	lerped=q1+delta;
	lerped.normalise();
	return lerped;
}

/*
Quaternion Quaternion::slerp(Quaternion q2,float timeStep) //Not realy correct, dear Watson
{
	Quaternion q1=Quaternion(w,v);
	Quaternion slerp;
	float angle=TVector::getAngle(q1.v,q2.v);
	angle=deg2rad(angle)/2;
	q1=q1.scalarMultiply(sin(angle*(1-timeStep)));
	q2=q2.scalarMultiply(sin(angle*timeStep));
	slerp=(q1+q2).scalarMultiply(1/sin(angle));
	slerp.normalise();
	return slerp;
}
*/
Quaternion Quaternion::slerp(Quaternion q2,float timeStep) {

	//This code was not developed by myself(F. N. Coada)
	//it was instead adapted from the solution described by Martin John Baker
	//at http://www.euclideanspace.com, using code from other participants to this website 


	// quaternion to return
	Quaternion slerp;

	Quaternion q1=Quaternion(w,v);
	// Calculate angle between them.
	double cosHalfTheta = q1.w * q2.w + q1.v._x * q2.v._x + q1.v._y * q2.v._y + q1.v._z * q2.v._z;

	//Since (w,x,y,z) and (-w,-x,-y,-z) represent the same rotation we should make sure that are not sensitive
	//to whether the positive of inverted form of the quaternion are used.
	if (cosHalfTheta < 0) 
	{
		q2.w = -q2.w; q2.v._x = -q2.v._x; q2.v._y = -q2.v._y; q2.v._z = q2.v._z;
		cosHalfTheta = -cosHalfTheta;
	}

	// if q1=q2 or q1=-q2 then theta = 0 and we can return q1
	if (abs(cosHalfTheta) >= 1.0){
		slerp.w = q1.w;slerp.v = q1.v;
		return slerp;
	}
	// Calculate temporary values.
	double halfTheta = acos(cosHalfTheta);
	double sinHalfTheta = sqrt(1.0 - cosHalfTheta*cosHalfTheta);
	// if theta = 180 degrees then result is not fully defined
	// we could rotate around any axis normal to q1 or q2
	if (fabs(sinHalfTheta) < 0.001){ // fabs is floating point absolute
		slerp.w = (q1.w * 0.5 + q2.w * 0.5);
		slerp.v._x = (q1.v._x * 0.5 + q2.v._x * 0.5);
		slerp.v._y = (q1.v._y * 0.5 + q2.v._y * 0.5);
		slerp.v._z = (q1.v._z * 0.5 + q2.v._z * 0.5);
		return slerp;
	}
	double ratioA = sin((1 - timeStep) * halfTheta) / sinHalfTheta;
	double ratioB = sin(timeStep * halfTheta) / sinHalfTheta; 
	//calculate Quaternion.
	slerp.w = (q1.w * ratioA + q2.w * ratioB);
	slerp.v._x = (q1.v._x * ratioA + q2.v._x * ratioB);
	slerp.v._y = (q1.v._y * ratioA + q2.v._y * ratioB);
	slerp.v._z = (q1.v._z * ratioA + q2.v._z * ratioB);
	return slerp;
}